//
// Created by gaoxiang on 19-5-4.
//
#include "myslam/visual_odometry.h"
#include <chrono>
#include "myslam/config.h"

namespace myslam {

VisualOdometry::VisualOdometry(std::string &config_path)
    : config_file_path_(config_path) {
        assert(Init() == true);
    }

bool VisualOdometry::Init() {
    // read from config file
    if (Config::SetParameterFile(config_file_path_) == false) {
        return false;
    }

    dataset_ = Dataset::Ptr(new Dataset(Config::Get<std::string>("dataset_dir")));
    CHECK_EQ(dataset_->Init(), true);

    // create components and links
    frontend_ = Frontend::Ptr(new Frontend);
    backend_ = Backend::Ptr(new Backend);           //单独一个线程Backend::BackendLoop() 
    map_ = Map::Ptr(new Map);
    viewer_ = Viewer::Ptr(new Viewer);          //单独一个线程Viewer::ThreadLoop()

    // frontend_->SetBackend(backend_);
    frontend_->SetMap(map_);
    frontend_->SetViewer(viewer_);
    frontend_->SetCameras(dataset_->GetCamera(0), dataset_->GetCamera(1));

    backend_->SetMap(map_);
    backend_->SetCameras(dataset_->GetCamera(0), dataset_->GetCamera(1));

    viewer_->SetMap(map_);

    visual_odometry_running_.store(true);
    visual_odometry_thread_ = std::thread(std::bind(&VisualOdometry::Run,this));
    success_flag = true;
    return true;
}

VisualOdometry::~VisualOdometry(){
    Visual_Odometry_Stop();
}

void VisualOdometry::Visual_Odometry_Stop(){
    
    visual_odometry_running_.store(false);
    visual_odometry_thread_.join();

    backend_->Stop();
    viewer_->Close();

    LOG(INFO) << "VO exit";
}

void VisualOdometry::Visual_Odometry_Update(cv::Mat* left_pic,cv::Mat* right_pic){
    if(success_flag==true){
        std::unique_lock<std::mutex> lock(data_mutex_);
        dataset_->image_left = *left_pic;
        dataset_->image_right = *right_pic;
        success_flag = false;
        visual_odometry_update_.notify_one();
    }
}

void VisualOdometry::Run() {
    while (visual_odometry_running_.load()) {
        std::unique_lock<std::mutex> lock(data_mutex_);
        visual_odometry_update_.wait(lock);

        LOG(INFO) << "VO is running";
        if (Step() == false) {
            break;
        }
        success_flag  = true;
    }
} 

bool VisualOdometry::Step() {
    Frame::Ptr new_frame = dataset_->NextFrame();   //读取左右图像然后resize()缩小四倍
    if (new_frame == nullptr) return false;

    auto t1 = std::chrono::steady_clock::now();
    bool success = frontend_->AddFrame(new_frame);  
    auto t2 = std::chrono::steady_clock::now();

    transfer_->Transfer_TransData(frontend_->current_pose_(0,0),
        frontend_->current_pose_(1,0),frontend_->current_pose_(2,0),frontend_->valid);
    auto time_used =
        std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1); 
    LOG(INFO) << "VO cost time: " << time_used.count() << " seconds.";
    return success;
}

}  // namespace manceyslam
